
#include "lcd.h"
#include "system.h"
#include "serial.h"
#include <stdio.h>
#include <stdarg.h>
#include <string.h>



/// Round up to next higher power of 2 (return x if it's already a power of 2).
size_t pow2ceil(size_t i)
{
    --i;
    i |= i >> 1;
    i |= i >> 2;
    i |= i >> 4;
    i |= i >> 8;

    return i+1;
}


Lcd::Lcd(Serial *_pSerial, unsigned int _uWidth, unsigned int _uHeight)
    :m_pSerial(_pSerial),
     m_uBufWidth(0),
     m_uBufHeightMask(0),
     m_pBuffer(NULL),
     m_uBufferSize(0),
     m_uLineCount(0),
     m_iX(0),
     m_iY(0),
     m_bBlink(false),
     m_uBlinkMillis(0)
{
    m_uBufWidth = std::max((unsigned int)16, _uWidth);
    m_uBufHeightMask = std::max((size_t)4, pow2ceil(_uHeight))-1;

    m_uBufferSize = (unsigned long)m_uBufWidth * (unsigned long)(m_uBufHeightMask+1);
    m_pBuffer = (char*)malloc(m_uBufferSize);
    memset(m_pBuffer, ' ', m_uBufferSize);

    setup();
}

Lcd::~Lcd()
{
    delete m_pSerial;
    free(m_pBuffer);
}

/// setup LCD
void Lcd::setup()
{
    m_pSerial->write(0x7C); // set LCD width (1)
    m_pSerial->write(4);    // set LCD width (2)
    m_pSerial->write(0x7C); // set LCD height (1)
    m_pSerial->write(6);    // set LCD height (1)
    SYSTEM::sleep(200);
    m_pSerial->write(0xFE); // switch cursor off (1)
    m_pSerial->write(0x0C); // switch cursor off (2)
    SYSTEM::sleep(200);
    m_pSerial->write(0xFE); // clear LCD (1)
    m_pSerial->write(0x01); // clear LCD (2)
    SYSTEM::sleep(200);
    m_pSerial->write(0x7C); // set LCD brightness (1)
    m_pSerial->write(140);  // set LCD brightness (2 : 128 - off; 157 - fully on)
    SYSTEM::sleep(20);
}

/// sets the backlight brightness (0 - 29)
void Lcd::setBacklight(unsigned char _uValue)
{
    m_pSerial->write(0x7C);           // set LCD brightness (1)
    m_pSerial->write(128 + _uValue);  // set LCD brightness (2 : 128 - off; 157 - fully on)
    SYSTEM::sleep(20);
}

/// update text on LCD based on x and y coordinates
void Lcd::scroll(int _ix, int _iy)
{
    if (_ix < 0) m_iX = 0;
    else if ((unsigned int)_ix > m_uBufWidth-16) m_iX = m_uBufWidth-16;
    else m_iX = _ix;

    if (_iy < 0) m_iY = 0;
    else if ((unsigned int)_iy > m_uBufHeightMask-1) m_iY = m_uBufHeightMask-1;
    else m_iY = _iy;

    // display bottom two lines
    unsigned int i1 = (m_uLineCount-2u-m_iY) & m_uBufHeightMask;
    unsigned int i2 = (m_uLineCount-1u-m_iY) & m_uBufHeightMask;

    // write line 1
    m_pSerial->write(0xFE);
    m_pSerial->write(0x80);
    m_pSerial->write((unsigned char*)m_pBuffer + (unsigned long)i1 * m_uBufWidth + (unsigned long)m_iX, 16);
    SYSTEM::sleep(50);

    // write line 2
    m_pSerial->write(0xFE);
    m_pSerial->write(0x80 | 0x40);
    m_pSerial->write((unsigned char*)m_pBuffer + (unsigned long)i2 * m_uBufWidth + (unsigned long)m_iX, 16);
    SYSTEM::sleep(50);
}

/// write a new line to LCD and scrolls previous line up
void Lcd::writeLine(const char *_pszFmt, ...)
{
    // clear line
    char *pLcdBufLine = m_pBuffer + (unsigned long)(m_uLineCount & m_uBufHeightMask) * m_uBufWidth;
    memset(pLcdBufLine, ' ', m_uBufWidth);

    // fill line with variable format string
    va_list ap;
    va_start(ap, _pszFmt);
    int n = vsnprintf(pLcdBufLine, m_uBufWidth, _pszFmt, ap);
    va_end(ap);

    // replace string terminating character '\0' with a ' '
    n = std::min(m_uBufWidth-1, (unsigned int)n);
    pLcdBufLine[n] = ' ';

    // scroll text
    m_uLineCount++;
    scroll(m_iX, m_iY);
}

// indicate activity on LCD
void Lcd::blink()
{
    if (SYSTEM::millis() > m_uBlinkMillis)
    {
        m_pSerial->write(0xFE);
        m_pSerial->write(0x8F);
        SYSTEM::sleep(20);

        if (m_bBlink == true) m_pSerial->write('O');
        else m_pSerial->write('*');
        SYSTEM::sleep(10);

        m_bBlink = !m_bBlink;
        m_uBlinkMillis = SYSTEM::millis() + 500;
    }
}


